
#include "../include/Distributions.hpp"
#include "../include/utils.hpp"
//#include "../include/Gibbs.hpp"
//#include "../include/SmcSampler.hpp"
#include "../include/Resample.hpp"
#include "../include/HMC_trunc.hpp"
#include "../include/OrthantBS.hpp"
#include "../include/Orthan.hpp"
#include "../include/Orthan2.hpp"
#include "../include/Orthantst.hpp"
#include "../include/Orthanghk.hpp"
#include "../include/Orthantsampler.hpp"
#include "../include/Orthantsampler2.hpp"
#include "../include/Orthanghkcache.hpp"
#include "../include/Misc/EP_tg.hpp"
#include "../include/Misc/EP_tg_sep.hpp"
#include "../include/Orthanghkst.hpp"
#include "../include/Orthantst_PF.hpp"
#include "../include/Misc/EP_ForwardProp.hpp"
#include "../include/Misc/BlockEP.hpp"
#include "../include/Misc/BlockEP_ntarget.hpp"
#include "../include/ForwardLooking.hpp"
extern "C"{

void sampler(int *M, int *p, double *pa, double *pX, double *pm,double *res)
{
	mat X=vect2mat<double>(pX,p[0],p[0]);
	mat A=chol(X);
	A=A.t();
	mat a=vect2mat<double>(pa,1,p[0]);
	mat m=vect2mat<double>(pm,1,p[0]);
	KernelHMC *K;
	a=a-m;
	Resample::SysResample *R;	//Resample::Multinomial *R;
	Orthant2::Orthant *O;
	Sampler2<KernelHMC,Resample::SysResample> *P;
//	cout << "hello";
	int numCPU = sysconf(_SC_NPROCESSORS_ONLN);
	//numCPU=2;
	//cout << "cpu  " << numCPU;
	double Z;	
	int i=0;
			
//	#pragma omp parallel for firstprivate(R,K,O,P), shared(Z)
//	for(int i=0;i<2;i++)
//	{	
		O=new Orthant2::Orthant(A,a);
		K=new KernelHMC(p[0],A,a);
		R=new Resample::SysResample;
		P=new Sampler2<KernelHMC,Resample::SysResample>(K,R,O,M[0],0.8);
		P->Filter();
		Z=P->Get_Z();	
		//delete DP;
		delete R;
		delete P;
		delete K; 
//	}

	double z=exp(Z);
	cout << "Int " << exp(Z);
	cout << " evidence: " << z;
	res[0]=z;
	cout << " " << p[0];
	cout << " " << a;
	/*	
	delete F;
//	delete DP;
	delete G;
	delete R;
	delete K;
	//delete P;	
*/
}
void sampler2(int *M, int *p, double *pa, double *pX, double *pm,double *res)
{
	mat X=vect2mat<double>(pX,p[0],p[0]);
	mat A=chol(X);
	A=A.t();
	mat a=vect2mat<double>(pa,1,p[0]);
	mat m=vect2mat<double>(pm,1,p[0]);
	KernelHMC *K;
	a=a-m;
	Resample::SysResample *R;	//Resample::Multinomial *R;
	Orthant::Orthant *O;
	Sampler<KernelHMC,Resample::SysResample> *P;
//	cout << "hello";
	int numCPU = sysconf(_SC_NPROCESSORS_ONLN);
	//numCPU=2;
	//cout << "cpu  " << numCPU;
	double Z;	
	int i=0;
			
//	#pragma omp parallel for firstprivate(R,K,O,P), shared(Z)
//	for(int i=0;i<2;i++)
//	{	
		O=new Orthant::Orthant(A,a,M[0]);
		K=new KernelHMC(p[0],A,a);
		R=new Resample::SysResample;
		P=new Sampler<KernelHMC,Resample::SysResample>(K,R,O,M[0],0.9);
		P->Filter();
		Z=P->Get_Z();	
		//delete DP;
		delete R;
		delete P;
		delete K; 
//	}

	double z=exp(Z);
	cout << "Int " << exp(Z);
	cout << " evidence: " << z;
	res[0]=z;
	cout << " " << p[0];
	cout << " " << a;
	/*	
	delete F;
//	delete DP;
	delete G;
	delete R;
	delete K;
	//delete P;	
*/
}


void sampler3(int *M, int *dim,double *L, double *trunpt, int *above, double *eta, int *tr,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	KernelHMC *K=new KernelHMC(dim[0],Lm,a);
//	
//	EP_tg EP;
//	mat m(dim[0],1);
//	m.fill(0);
//	mat Sig=Lm*Lm.t();
//	EP(m,Sig,a.t(),2,0.5);
//	cout << EP.Get_Mu();
//	cout << EP.Get_Sig();
	Resample::SysResample *R=new Resample::SysResample;
	Ghk<Resample::SysResample,KernelHMC> G(R,K);
	G.ghk_oneside(L,trunpt,above,dim,eta,M,res,*tr);
	delete R;
	delete K;

}
int test_tr(mat A,mat B)
{
	int res=1;
	int d=B.n_elem;
	for(int i=0;i<d;i++)
	{
		if(A(0,i)<B(0,i))
		{
			res=0;
		}
	}
	return res;
}

void IS(int *M, int *dim,double *L, double *trunpt,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	EP_tg EP;
	mat m(dim[0],1);
	m.fill(0);
	mat Sig=Lm*Lm.t();
	EP(m,Sig,a.t(),1,1);
	mat Muh=EP.Get_Mu();
	mat Sigh=EP.Get_Sig();
	Distribution::Gaussian G1(dim[0],Muh,Sigh);
	Distribution::Gaussian G2(dim[0],m,Sig);
	double sum;
	mat temp;
	int first=1;
	int k=0;
	while(first)
	{
		temp=G1.r(1);
		if(test_tr(temp.t(),a))
		{
			sum=G2.d(temp,1,0)-G1.d(temp,1,0);
			first=0;
		}
		k++;
	}
	for(int i=k;i<*M;i++)
	{
		temp=G1.r(1);
		if(test_tr(temp.t(),a))
		{
			sum=log_add(G2.d(temp,1,0)-G1.d(temp,1,0),sum);			
		}
	}
	*res=sum-log(*M);
}
void EPtrunc(int *dim,double *L, double *trunpt,double *mu,double *sig)	
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	EP_tg_sep EP;
	mat m(dim[0],1);
	m.fill(0);
	mat Sig=Lm*Lm.t();
	EP(m,Sig,a.t(),2,1);
	mat Muh=EP.Get_Mu();
	mat Sigh=EP.Get_Sig();
	mat2vect<double>(Muh,*dim,1,mu);
	mat2vect<double>(Sigh,*dim,*dim,sig);

}

/*
void sampler4(int *M, int *dim,double *L, double *trunpt, int *above, double *eta,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	KernelHMC *K=new KernelHMC(dim[0],Lm,a);
	Resample::SysResample *R=new Resample::SysResample;
	Ghkc<Resample::SysResample,KernelHMC> G(R,K);
	G.ghk_oneside(L,trunpt,above,dim,eta,M,res);
	delete R;
	delete K;

}
*/
void ghk_oneside(double *L, double* trunpt, int *above, int *dim, int *n, double *res)
/*	routine to implement ghk with a region defined by truncation only on one- side
 						r mcculloch 8/04
        if above=1, then we truncate component i from above at point trunpt[i-1]
        L is lower triangular root of Sigma
	random vector is assumed to have zero mean
    	n is number of draws to use in GHK	
	modified 6/05 by rossi to check arg into qnorm
*/
{
   boost::random::uniform_01<> U;
   RandomG::Random<boost::random::uniform_01<> > *_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
   double* prod=new double[*n]; 
   int i,j,k;
   for(int i=0;i<*n;i++)
   {
	   prod[i]=0;
   }
   double mu,tpz,u,pa,pb,arg;
   mat z(*n,*dim) ;
  for(j=0;j<*dim;j++) {
    for(i=0;i<*n;i++) {
         mu=0.0; for(k=0;k<j;k++) mu += L[k*(*dim)+j]*z(i,k);
	 tpz = (trunpt[j]-mu)/L[j*(*dim)+j];
	 if(above[j]) {
	    pa=0.0; pb = pnorm(tpz,0.0,1.0,1,0);   
	 }
	 else {
	    pb=1.0; pa = pnorm(tpz,0.0,1.0,1,0);
	 }
		 prod[i]+=log(pb-pa);
	//	cout << prod[i] << " "; 
	u = (*_runif)();
	 arg=u*pb+(1.-u)*pa;
	 if(arg > .999999999) arg=.999999999;
	 if(arg < .0000000001) arg=.0000000001;
	 z(i,j) = qnorm(arg,0.0,1.0,1,0);
      }
   }
  *res=prod[0];
  for(int k=1;k<*n;k++)
  {
	  if(exp(prod[k])!=0)
	  {
		   *res=log_add(prod[k],*res);
	  }
  }
  *res=*res-log(*n);
   delete _runif;
   delete[] prod;
}

//////////////////////////////////////////////////////////////////////

void ghk_order(double *L, double* trunpt, int *above, int *dim, double *res)
{
   int i,j,k;
   double prod=1;
   double mu,tpz,u,pa,pb,arg;
   mat z(1,*dim) ;
  for(j=0;j<*dim;j++) {
         mu=0.0; 
	 for(k=0;k<j;k++) mu += L[k*(*dim)+j]*z(0,k);
	 tpz = (trunpt[j]-mu)/L[j*(*dim)+j];	 
	 pa = pnorm(tpz,0.0,1.0,1,0);   
	 prod*=1-pa;
	//cout << prod << " "; 
	 z(0,j) = dnorm(tpz,0,1,0)/(1-pnorm(tpz,0.0,1.0,1,0));
//	cout << z(0,j) << " "; 
   }
  *res=prod;
}


void Substitute(double *X, double *Y, int *p, int *k1, int *k2)
{
	*k1=*k1-1;
	*k2=*k2-1;
	int h=*k2-*k1;
	int ii=0;
	int jj=0;
	for(int i=0;i<*p;i++)
	{
		for(int j=0;j<*p;j++)
		{
			ii=i;
			jj=j;

			if(i==*k1)
			{
				ii=*k2;

			}else if(i==*k2)
			{
				ii=*k1;
			}
			if(j==*k1)
			{
				jj=*k2;

			}else if(j==*k2)
			{
				jj=*k1;
			}	
			Y[i+(*p)*j]=X[ii+(*p)*jj];	

		}
	}

}

void sampler4(int *M, int *dim,double *L, double *trunpt, int *above, double *eta, int *depth,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	Resample::SysResample *R=new Resample::SysResample;
	GhkBS<Resample::SysResample> G(R);
	G.ghk_oneside(L,trunpt,above,dim,eta,M,depth,res);
	delete R;

}

void samplerFL(int *M, int *dim,double *L, double *trunpt, int *above, double *eta, int *tr,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	KernelHMC *K=new KernelHMC(dim[0],Lm,a);
	BlockEP *EP=new BlockEP(Lm,a);
//	mat m(dim[0],1);
//	m.fill(0);
//	cout << EP.Get_Mu();
//	cout << EP.Get_Sig();
	Resample::SysResample *R=new Resample::SysResample;
	GhkFL<Resample::SysResample,KernelHMC,BlockEP> G(R,K,EP);
	G.ghk_oneside(L,trunpt,above,dim,eta,M,res,*tr);
	delete R;
	delete K;

}


//GHK ofr student distrib

//separeted
void sampler5(int *M, int *dim,double *L, double *trunpt, int *nu, double *eta,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	Orthantst G(L,trunpt,*dim, nu[0]);
	G(*M,res);
	cout << "final" <<*res;
}
//mixture representation
void sampler6(int *M, int *dim,double *L, double *trunpt,int *nu, int *above, double *eta, int *tr,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	KernelHMC *K=new KernelHMC(dim[0],Lm,a);
	Resample::SysResample *R=new Resample::SysResample;
	Ghkst<Resample::SysResample,KernelHMC> G(R,K);
	G.ghk_oneside(L,trunpt,nu,above,dim,eta,M,res,*tr);
	
	delete R;
	delete K;

}
void sampler7(int *M, int *dim,double *L, double *trunpt, int *nu, double *eta,double *res)
{
	mat Lm=vect2mat<double>(L,dim[0],dim[0]);
	mat a=vect2mat<double>(trunpt,1,dim[0]);
	Resample::SysResample *R=new Resample::SysResample;
	Orthantst_PF<Resample::SysResample> G(L,trunpt,*dim, nu[0],R);
	G(*M,res,eta);
	delete R;
}

void testbrt(void)
{
	mat A(2,2);
	A(0,1)=0.2;
	A(1,1)=1;
	A(1,0)=0.2;
	A(0,0)=1;
	mat a(2,1);
	a.fill(0.2);
	a(0,0)=1.1;
	cout << A;
	Birtnorm Br;
	mat X=Br(A,a);
	cout << X;
}
}
